/* main.c - Application main entry point */

/*
 * Copyright (c) 2015-2016 Intel Corporation
 *
 * SPDX-License-Identifier: Apache-2.0
 */

#include <zephyr/types.h>
#include <stddef.h>
#include <errno.h>
#include <zephyr/kernel.h>
#include <zephyr/sys/printk.h>
#include <zephyr/logging/log.h>
#include <zephyr/sys/slist.h>

#include <zephyr/bluetooth/bluetooth.h>
#include <zephyr/bluetooth/hci.h>
#include <zephyr/bluetooth/conn.h>
#include <zephyr/bluetooth/uuid.h>
#include <zephyr/bluetooth/gatt.h>
#include <zephyr/sys/byteorder.h>

// #include "gatt_cache.h"
#include "ble_multi_central.h"

LOG_MODULE_REGISTER(bt_central, LOG_LEVEL_INF);

#define MAX_CONNECTED_DEVICES 8

#define AUTO_RECONNECT_INTERVAL K_SECONDS(5)

struct gatt_descriptor
{
    struct bt_uuid *uuid;
    uint16_t handle;
    sys_snode_t node;
};

struct gatt_characteristic
{
    struct bt_uuid *uuid;
    uint16_t handle;
    uint16_t ccc_handle;
    uint8_t properties;
    struct bt_gatt_subscribe_params subscribe_params; // **每个特征值独立的订阅参数**
    sys_slist_t descriptors;                          // **存储该特征值下的所有 Descriptor**
    sys_snode_t node;
};

struct gatt_service
{
    struct bt_uuid *uuid;
    uint16_t start_handle;
    uint16_t end_handle;
    sys_slist_t characteristics; // **存储该 Service 下的所有 Characteristic**
    sys_snode_t node;
};

struct bt_device
{
    struct bt_conn *conn;
    bt_addr_le_t addr;
    struct bt_gatt_discover_params discover_params; // 用于服务发现
    sys_slist_t gatt_services;                      // **存储所有发现的 Service**
    sys_snode_t node;
    bool connected;
    bool gatt_discovery_started; // **标志位，避免重复触发 GATT 发现**
    bool mtu_exchanged;          // **是否已完成 MTU 交换**
};

static sys_slist_t device_list;
static struct bt_conn *default_conn;
static uint16_t prev_end_handle = 0;
static struct k_work_delayable reconnect_work;
static struct k_work_delayable mtu_timeout_work;
static struct bt_gatt_discover_params discover_params;
static struct bt_gatt_subscribe_params subscribe_params;
static struct k_work my_work;

static struct bt_gatt_subscribe_params subscribe_params_test;

static void scan_result(const struct bt_le_scan_recv_info *info,
                        struct net_buf_simple *buf);
static void scan_timeout(void);
int bt_central_scan_start(void);
int bt_central_scan_stop(void);
int bt_central_connect(const bt_addr_le_t *addr);
int bt_central_disconnect(const bt_addr_le_t *addr);
static void connected(struct bt_conn *conn, uint8_t err);
static void disconnected(struct bt_conn *conn, uint8_t reason);
static bool le_param_req(struct bt_conn *conn, struct bt_le_conn_param *param);
static void le_param_updated(struct bt_conn *conn, uint16_t interval, uint16_t latency,
                             uint16_t timeout);
static void le_phy_updated(struct bt_conn *conn,
                           struct bt_conn_le_phy_info *param);
static void le_data_len_updated(struct bt_conn *conn,
                                struct bt_conn_le_data_len_info *info);
static void mtu_updated(struct bt_conn *conn, uint16_t tx, uint16_t rx);
static void request_le_data_len_update(struct bt_conn *conn);
static uint8_t discover_service_cb(struct bt_conn *conn, const struct bt_gatt_attr *attr,
                                   struct bt_gatt_discover_params *params);
static uint8_t discover_char_cb(struct bt_conn *conn, const struct bt_gatt_attr *attr,
                                struct bt_gatt_discover_params *params);
static void start_service_discovery(struct bt_conn *conn);
static void subscribe_notify(struct bt_device *dev, struct gatt_characteristic *gatt_char);
int bt_central_enable_notify(const bt_addr_le_t *addr, struct bt_uuid *uuid, bool enable);
static uint8_t notify_cb(struct bt_conn *conn, struct bt_gatt_subscribe_params *params,
                         const void *data, uint16_t length);

static const struct bt_uuid *default_notify_uuids[] = {
    // BT_UUID_DECLARE_16(0xFF02), // Custom UUID
    // BT_UUID_DECLARE_16(0xFF03), // Custom UUID
    // BT_UUID_DECLARE_128(BT_UUID_128_ENCODE(0x0000ff02, 0x4c72, 0x4dd5, 0xb86f, 0x412340a15dad)),
    BT_UUID_DECLARE_128(BT_UUID_128_ENCODE(0x0000ff03, 0x4c72, 0x4dd5, 0xb86f, 0x412340a15dad)),
};

#define BT_UUID_NB_DATA_SERVICE \
    BT_UUID_DECLARE_128(BT_UUID_128_ENCODE(0x0000ff01, 0x3c17, 0xd293, 0x8e48, 0x14fe2e4da212))

#define BT_UUID_NB_DATA_SERVICE_NOTIFY \
    BT_UUID_DECLARE_128(BT_UUID_128_ENCODE(0x0000ff03, 0x3c17, 0xd293, 0x8e48, 0x14fe2e4da212))

#define BT_UUID_NB_DATA_SERVICE_WRITE \
    BT_UUID_DECLARE_128(BT_UUID_128_ENCODE(0x0000ff02, 0x3c17, 0xd293, 0x8e48, 0x14fe2e4da212))

static struct bt_uuid_128 discover_uuid = BT_UUID_INIT_128(0);

#define DEFAULT_NOTIFY_UUID_COUNT (sizeof(default_notify_uuids) / sizeof(default_notify_uuids[0]))

static struct bt_le_scan_cb scan_cb = {
    .recv = scan_result,
    .timeout = scan_timeout,
};

static struct bt_conn_cb conn_cb = {
    .connected = connected,
    .disconnected = disconnected,
    .le_param_req = le_param_req,
    .le_param_updated = le_param_updated,
    .le_phy_updated = le_phy_updated,
    .le_data_len_updated = le_data_len_updated,
};

static struct bt_gatt_cb gatt_cb = {
    .att_mtu_updated = mtu_updated,
};

static void discovery_complete_cb(struct bt_conn *conn, int err)
{
    if (err)
    {
        printk("Failed to discover service (err %d)\n", err);
        return;
    }
    printk("Discover complete, conn %p\n", conn);
}

static int bt_central_enable_notify_test(struct bt_conn *conn, bool enable)
{
    // struct bt_gatt_subscribe_params subscribe_params = {0};
    memset(&subscribe_params_test, 0, sizeof(subscribe_params_test));

    subscribe_params_test.notify = notify_cb;
    subscribe_params_test.value = enable ? BT_GATT_CCC_NOTIFY : 0;
    subscribe_params_test.ccc_handle = 0x0013;
    subscribe_params_test.value_handle = 0x0012;

    int err = enable ? bt_gatt_subscribe(conn, &subscribe_params_test)
                     : bt_gatt_unsubscribe(conn, &subscribe_params_test);

    if (err)
    {
        LOG_ERR("Failed to %s Notify (err %d)", enable ? "enable" : "disable", err);
        return err;
    }

    LOG_INF("%s Notify for UUID", enable ? "Enabled" : "Disabled");
    return 0;
}

void my_work_handler(struct k_work *work)
{
    LOG_INF("Executing k_work task...");

    // bt_central_enable_notify_test(default_conn, true);
    bt_conn_mgr_notify_enable(default_conn, true, notify_cb);
    k_sleep(K_SECONDS(1));

    uint8_t cmd_buffer[] = {0x8F, 0xD7, 0x01, 0x00, 0x01, 0x00};
    bt_conn_mgr_send_data(default_conn, cmd_buffer, sizeof(cmd_buffer));
    // bt_central_enable_notify_test(default_conn, false);
    // k_sleep(K_SECONDS(1));
    // bt_central_enable_notify_test(default_conn, true);
}

static bool is_uuid_in_list(const struct bt_uuid *uuid, const struct bt_uuid *uuid_list[], size_t count)
{
    if (!uuid)
        return false;

    // **转换 UUID 为 16-bit 形式**
    struct bt_uuid_16 *uuid16 = (struct bt_uuid_16 *)BT_UUID_16(uuid);

    if (!uuid16)
        return false; // **UUID 不是 16-bit 兼容的，直接返回 false**

    for (size_t i = 0; i < count; i++)
    {
        if (!bt_uuid_cmp((struct bt_uuid *)uuid16, uuid_list[i]))
        {
            return true;
        }
    }

    return false;
}

static void reconnect_attempt(struct k_work *work)
{
    struct bt_device *dev;
    SYS_SLIST_FOR_EACH_CONTAINER(&device_list, dev, node)
    {
        if (!dev->connected)
        {
            if (dev->conn)
            {
                LOG_WRN("Conn still exists, forcing release");
                bt_conn_unref(dev->conn);
                dev->conn = NULL;
            }

            LOG_INF("Reconnecting to device: %02X:%02X:%02X:%02X:%02X:%02X",
                    dev->addr.a.val[5], dev->addr.a.val[4], dev->addr.a.val[3],
                    dev->addr.a.val[2], dev->addr.a.val[1], dev->addr.a.val[0]);

            bt_central_connect(&dev->addr);
        }
    }
    k_work_schedule(&reconnect_work, AUTO_RECONNECT_INTERVAL);
}

static struct bt_device *find_device(const bt_addr_le_t *addr)
{
    struct bt_device *dev;
    SYS_SLIST_FOR_EACH_CONTAINER(&device_list, dev, node)
    {
        if (!bt_addr_le_cmp(&dev->addr, addr))
        {
            return dev;
        }
    }
    return NULL;
}

static void print_gatt_info(struct bt_device *dev)
{
    LOG_INF("===========================================");
    char addr_str[BT_ADDR_LE_STR_LEN];
    bt_addr_le_to_str(&dev->addr, addr_str, sizeof(addr_str));
    LOG_INF("GATT Information for Device: %s", addr_str);
    LOG_INF("-------------------------------------------");

    struct gatt_service *service;
    SYS_SLIST_FOR_EACH_CONTAINER(&dev->gatt_services, service, node)
    {
        char uuid_str[BT_UUID_STR_LEN];
        bt_uuid_to_str(service->uuid, uuid_str, sizeof(uuid_str));
        LOG_INF("Service: UUID %s, Handle Range [0x%04X - 0x%04X]",
                uuid_str, service->start_handle, service->end_handle);

        struct gatt_characteristic *ch;
        SYS_SLIST_FOR_EACH_CONTAINER(&service->characteristics, ch, node)
        {
            memset(uuid_str, 0, sizeof(uuid_str));
            bt_uuid_to_str(ch->uuid, uuid_str, sizeof(uuid_str));
            LOG_INF("  ├── Characteristic: UUID %s, Handle 0x%04X",
                    uuid_str, ch->handle);

            struct gatt_descriptor *desc;
            SYS_SLIST_FOR_EACH_CONTAINER(&ch->descriptors, desc, node)
            {
                memset(uuid_str, 0, sizeof(uuid_str));
                bt_uuid_to_str(desc->uuid, uuid_str, sizeof(uuid_str));
                LOG_INF("    ├── Descriptor: UUID %s, Handle 0x%04X",
                        uuid_str, desc->handle);
            }
        }
    }
    LOG_INF("===========================================");
}

static void mtu_timeout_handler(struct k_work *work)
{
    LOG_WRN("MTU exchange timeout, proceeding with GATT discovery...");

    struct bt_device *dev;
    SYS_SLIST_FOR_EACH_CONTAINER(&device_list, dev, node)
    {
        if (dev->connected && !dev->gatt_discovery_started)
        {
            char addr_str[BT_ADDR_LE_STR_LEN];
            bt_addr_le_to_str(&dev->addr, addr_str, sizeof(addr_str));
            LOG_INF("Starting GATT discovery for device: %s",
                    addr_str);
            dev->gatt_discovery_started = true;
            start_service_discovery(dev->conn);
        }
    }
}

// static struct gatt_characteristic *find_gatt_char(struct bt_device *dev, struct bt_uuid *uuid)
// {
//     struct gatt_characteristic *gatt_char;
//     SYS_SLIST_FOR_EACH_CONTAINER(&dev->gatt_chars, gatt_char, node)
//     {
//         if (!bt_uuid_cmp(gatt_char->uuid, uuid))
//         {
//             return gatt_char;
//         }
//     }
//     return NULL;
// }

int bt_central_get_services(const bt_addr_le_t *addr)
{
    struct bt_device *dev = find_device(addr);
    if (!dev)
        return -ENOTCONN;

    LOG_INF("Listing GATT Services:");
    struct gatt_service *service;
    SYS_SLIST_FOR_EACH_CONTAINER(&dev->gatt_services, service, node)
    {
        char uuid_str[BT_UUID_STR_LEN];
        bt_uuid_to_str(service->uuid, uuid_str, sizeof(uuid_str));
        LOG_INF("  Service: UUID %s, Handle Range [0x%04X - 0x%04X]",
                uuid_str, service->start_handle, service->end_handle);
    }

    return 0;
}

int bt_central_get_characteristics(const bt_addr_le_t *addr, struct bt_uuid *service_uuid)
{
    struct bt_device *dev = find_device(addr);
    if (!dev)
        return -ENOTCONN;

    struct gatt_service *target_service = NULL;
    struct gatt_service *service;
    SYS_SLIST_FOR_EACH_CONTAINER(&dev->gatt_services, service, node)
    {
        if (!bt_uuid_cmp(service->uuid, service_uuid))
        {
            target_service = service;
            break;
        }
    }

    if (!target_service)
    {
        LOG_ERR("Service UUID not found");
        return -EINVAL;
    }

    char uuid_str[BT_UUID_STR_LEN];
    bt_uuid_to_str(service_uuid, uuid_str, sizeof(uuid_str));
    LOG_INF("Listing Characteristics for Service UUID: %s", uuid_str);
    struct gatt_characteristic *ch;
    SYS_SLIST_FOR_EACH_CONTAINER(&target_service->characteristics, ch, node)
    {
        char uuid_str[BT_UUID_STR_LEN];
        bt_uuid_to_str(ch->uuid, uuid_str, sizeof(uuid_str));
        LOG_INF("  Characteristic: UUID %s, Handle 0x%04X", uuid_str, ch->handle);
    }

    return 0;
}

int bt_central_send_data(const bt_addr_le_t *addr, struct bt_uuid *uuid, const uint8_t *data, uint16_t len)
{
    struct bt_device *dev = find_device(addr);
    if (!dev)
        return -ENOTCONN;

    struct gatt_service *service;
    struct gatt_characteristic *gatt_char = NULL;

    // **查找 UUID 对应的 Characteristic**
    SYS_SLIST_FOR_EACH_CONTAINER(&dev->gatt_services, service, node)
    {
        SYS_SLIST_FOR_EACH_CONTAINER(&service->characteristics, gatt_char, node)
        {
            if (!bt_uuid_cmp(gatt_char->uuid, uuid))
            {
                break;
            }
        }
    }

    if (!gatt_char)
    {
        LOG_ERR("Characteristic UUID not found");
        return -EINVAL;
    }

    if (!(gatt_char->properties & BT_GATT_CHRC_WRITE_WITHOUT_RESP))
    {
        LOG_ERR("Characteristic does not support write without response");
        return -EINVAL;
    }

    int err = bt_gatt_write_without_response(dev->conn, gatt_char->handle, data, len, false);
    if (err)
    {
        LOG_ERR("Failed to send data (err %d)", err);
        return err;
    }

    char uuid_str[BT_UUID_STR_LEN];
    bt_uuid_to_str(uuid, uuid_str, sizeof(uuid_str));
    LOG_INF("Sent data to UUID: %s", uuid_str);
    return 0;
}

static uint8_t notify_cb(struct bt_conn *conn, struct bt_gatt_subscribe_params *params,
                         const void *data, uint16_t length)
{
    if (data && length > 0)
    {
        LOG_INF("Received notification, length: %d", length);
        // uart_send((const uint8_t *)data, length);
    }

    return 0;
}

// static void subscribe_notify(struct bt_device *dev, struct gatt_characteristic *gatt_char)
// {
//     if (!(gatt_char->properties & BT_GATT_CHRC_NOTIFY))
//     {
//         LOG_WRN("Characteristic does not support Notify, skipping subscription");
//         return;
//     }

//     memset(&dev->subscribe_params, 0, sizeof(dev->subscribe_params));

//     dev->subscribe_params.notify = notify_cb;
//     dev->subscribe_params.value = BT_GATT_CCC_NOTIFY;
//     dev->subscribe_params.ccc_handle = gatt_char->handle + 1; // **通常 CCCD 在特征值后面**
//     dev->subscribe_params.value_handle = gatt_char->handle;

//     int err = bt_gatt_subscribe(dev->conn, &dev->subscribe_params);
//     if (err)
//     {
//         LOG_ERR("Failed to subscribe to Notify (err %d)", err);
//     }
//     else
//     {
//         char uuid_str[BT_UUID_STR_LEN];
//         bt_uuid_to_str(gatt_char->uuid, uuid_str, sizeof(uuid_str));
//         LOG_INF("Subscribed to Notify UUID: %s", uuid_str);
//     }
// }

static uint8_t discover_cccd_cb(struct bt_conn *conn, const struct bt_gatt_attr *attr,
                                struct bt_gatt_discover_params *params)
{
    struct bt_device *dev = find_device(bt_conn_get_dst(conn));
    if (!dev)
    {
        LOG_ERR("iscover_cccd_cb: NO device found.");
        return BT_GATT_ITER_STOP;
    }

    if (!attr)
    {
        LOG_INF("Descriptor discovery completed.");
        // print_gatt_info(dev);
        return BT_GATT_ITER_STOP;
    }

    // struct bt_device *dev = find_device(bt_conn_get_dst(conn));
    struct gatt_service *service;
    struct gatt_characteristic *gatt_char;

    SYS_SLIST_FOR_EACH_CONTAINER(&dev->gatt_services, service, node)
    {
        SYS_SLIST_FOR_EACH_CONTAINER(&service->characteristics, gatt_char, node)
        {
            if (gatt_char->handle + 1 == attr->handle)
            {
                struct gatt_descriptor *desc = k_malloc(sizeof(*desc));
                if (!desc)
                    return BT_GATT_ITER_CONTINUE;

                // desc->uuid = (struct bt_uuid *)attr->uuid;
                desc->uuid = k_malloc(sizeof(struct bt_uuid_128));
                if (desc->uuid)
                {
                    memcpy(desc->uuid, attr->uuid, sizeof(struct bt_uuid_128));
                }
                else
                {
                    LOG_ERR("Failed to allocate memory for descriptor UUID");
                }
                printk("attr uuid type:%d\n", attr->uuid->type);

                desc->handle = attr->handle;
                sys_slist_append(&gatt_char->descriptors, &desc->node);

                char uuid_str[BT_UUID_STR_LEN];
                bt_uuid_to_str(desc->uuid, uuid_str, sizeof(uuid_str));
                LOG_INF("    ├── Descriptor: UUID %s, Handle 0x%04X",
                        uuid_str, attr->handle);

                // **检查是否是 `0x2902` (CCCD)**
                if (!bt_uuid_cmp(desc->uuid, BT_UUID_GATT_CCC))
                {
                    gatt_char->ccc_handle = desc->handle; // **存储 CCC 句柄**
                    char uuid_str[BT_UUID_STR_LEN];
                    bt_uuid_to_str(gatt_char->uuid, uuid_str, sizeof(uuid_str));
                    LOG_INF("Found CCCD (0x2902) for Characteristic:: %s", uuid_str);
                }

                // **如果该 Characteristic 需要自动开启 Notify，则订阅**
                for (int i = 0; i < DEFAULT_NOTIFY_UUID_COUNT; i++)
                {
                    // TODO
                    if (!bt_uuid_cmp(gatt_char->uuid, default_notify_uuids[i]))
                    {
                        char uuid_str[BT_UUID_STR_LEN];
                        bt_uuid_to_str(gatt_char->uuid, uuid_str, sizeof(uuid_str));
                        LOG_INF("Automatically enabling Notify for UUID: %s",
                                uuid_str);

                        memset(&gatt_char->subscribe_params, 0, sizeof(gatt_char->subscribe_params));
                        gatt_char->subscribe_params.notify = notify_cb;
                        gatt_char->subscribe_params.value = BT_GATT_CCC_NOTIFY;
                        gatt_char->subscribe_params.ccc_handle = desc->handle + 1; // TODO
                        gatt_char->subscribe_params.value_handle = gatt_char->handle + 1;

                        printk("[ATTRIBUTE] handle 0x%04X\n", attr->handle);
                        printk("value handle: 0x%04X, ccc handle: 0x%04X\n", gatt_char->handle + 1, desc->handle + 1);
                        int err = bt_gatt_subscribe(dev->conn, &gatt_char->subscribe_params);
                        if (err)
                        {
                            LOG_ERR("Failed to subscribe to Notify (err %d)", err);
                        }
                        else
                        {
                            char uuid_str[BT_UUID_STR_LEN];
                            bt_uuid_to_str(gatt_char->uuid, uuid_str, sizeof(uuid_str));
                            LOG_INF("Notify enabled for UUID: %s", uuid_str);
                        }
                    }
                }
            }
        }
    }

    return BT_GATT_ITER_CONTINUE;
}

static uint8_t discover_char_cb(struct bt_conn *conn, const struct bt_gatt_attr *attr,
                                struct bt_gatt_discover_params *params)
{
    struct bt_device *dev = find_device(bt_conn_get_dst(conn));
    if (!dev)
    {
        LOG_ERR("Device not found, possible memory corruption!");
        return BT_GATT_ITER_STOP;
    }

    // printk("[CHAR ATTRIBUTE] handle 0x%04X\n", attr->handle);

    if (!attr)
    {
        // 查找完char就开始查找descriptor
        LOG_INF("Characteristic discovery completed, starting descriptor discovery...");

#if 0
        struct gatt_service *service;
        struct gatt_characteristic *first_char = NULL;

        SYS_SLIST_FOR_EACH_CONTAINER(&dev->gatt_services, service, node)
        {
            SYS_SLIST_FOR_EACH_CONTAINER(&service->characteristics, first_char, node)
            {
                break; // 只取第一个 characteristic 进行 descriptor 发现
            }

            if (first_char)
            {
                struct bt_gatt_discover_params *desc_params = k_malloc(sizeof(*desc_params));
                if (!desc_params)
                    return BT_GATT_ITER_STOP;

                memset(desc_params, 0, sizeof(*desc_params));
                desc_params->uuid = NULL;
                desc_params->func = discover_cccd_cb;
                desc_params->start_handle = first_char->handle + 1;
                desc_params->end_handle = service->end_handle; // 先使用 service->end_handle，之后发现过程中修正
                desc_params->type = BT_GATT_DISCOVER_DESCRIPTOR;
                bt_gatt_discover(conn, desc_params);
            }
            // {
            //     struct bt_gatt_discover_params *desc_params = k_malloc(sizeof(*desc_params));
            //     if (!desc_params)
            //         continue;

            //     memset(desc_params, 0, sizeof(*desc_params));
            //     desc_params->uuid = NULL;
            //     desc_params->func = discover_cccd_cb;
            //     desc_params->start_handle = gatt_char->handle + 1;
            //     desc_params->end_handle = service->end_handle;
            //     desc_params->type = BT_GATT_DISCOVER_DESCRIPTOR;
            //     bt_gatt_discover(conn, desc_params);
            // }
        }
#endif
        return BT_GATT_ITER_STOP;
    }

    struct bt_gatt_chrc *chrc = (struct bt_gatt_chrc *)attr->user_data;
    if (!chrc)
        return BT_GATT_ITER_CONTINUE;

    struct gatt_service *gatt_service = CONTAINER_OF(sys_slist_peek_tail(&dev->gatt_services),
                                                     struct gatt_service, node);
    if (!gatt_service)
        return BT_GATT_ITER_CONTINUE;

    struct gatt_characteristic *gatt_char = k_malloc(sizeof(*gatt_char));
    if (!gatt_char)
        return BT_GATT_ITER_CONTINUE;

    gatt_char->uuid = k_malloc(sizeof(struct bt_uuid_128));
    if (gatt_char->uuid)
    {
        memcpy(gatt_char->uuid, chrc->uuid, sizeof(struct bt_uuid_128));
    }
    else
    {
        LOG_ERR("Failed to allocate memory for characteristic UUID");
    }

    gatt_char->handle = attr->handle;
    // gatt_char->uuid = chrc->uuid;
    gatt_char->properties = chrc->properties;
    gatt_char->ccc_handle = 0;
    sys_slist_init(&gatt_char->descriptors);
    sys_slist_append(&gatt_service->characteristics, &gatt_char->node);

    char uuid_str[BT_UUID_STR_LEN];
    bt_uuid_to_str(gatt_char->uuid, uuid_str, sizeof(uuid_str));
    LOG_INF("  ├── Characteristic: UUID %s, Handle 0x%04X",
            uuid_str, attr->handle);

    return BT_GATT_ITER_CONTINUE;
}

static uint8_t discover_service_cb(struct bt_conn *conn, const struct bt_gatt_attr *attr,
                                   struct bt_gatt_discover_params *params)
{
    struct bt_device *dev = find_device(bt_conn_get_dst(conn));
    if (!dev)
    {
        LOG_ERR("NO Device found.");
        return BT_GATT_ITER_STOP;
    }

    // static uint16_t prev_end_handle = 0; // **存储上一个服务的 end_handle**

    if (!attr)
    {
        // 查找service完成就开始查找char
        LOG_INF("Service discovery completed, starting characteristic discovery...");
        dev->gatt_discovery_started = false; // **GATT 发现完成，重置标志**

        // **修正最后一个服务的 end_handle**
        struct gatt_service *last_service = CONTAINER_OF(sys_slist_peek_tail(&dev->gatt_services),
                                                         struct gatt_service, node);
        if (last_service && last_service->end_handle == 0xFFFF)
        {
            last_service->end_handle = prev_end_handle;
        }

        memset(&dev->discover_params, 0, sizeof(dev->discover_params));
        dev->discover_params.uuid = NULL;
        dev->discover_params.func = discover_char_cb;
        dev->discover_params.start_handle = 0x0001;
        dev->discover_params.end_handle = 0xFFFF;
        dev->discover_params.type = BT_GATT_DISCOVER_CHARACTERISTIC;
        return bt_gatt_discover(conn, &dev->discover_params);

        // {
        //     struct gatt_service *service;
        //     struct gatt_characteristic *first_char = NULL;

        //     SYS_SLIST_FOR_EACH_CONTAINER(&dev->gatt_services, service, node)
        //     {
        //         SYS_SLIST_FOR_EACH_CONTAINER(&service->characteristics, first_char, node)
        //         {
        //             break;
        //         }

        //         if (first_char)
        //         {
        //             struct bt_gatt_discover_params *desc_params = k_malloc(sizeof(*desc_params));
        //             if (!desc_params)
        //                 return BT_GATT_ITER_STOP;

        //             memset(desc_params, 0, sizeof(*desc_params));
        //             desc_params->uuid = NULL;
        //             desc_params->func = discover_cccd_cb;
        //             desc_params->start_handle = first_char->handle + 1;
        //             desc_params->end_handle = service->end_handle;
        //             desc_params->type = BT_GATT_DISCOVER_DESCRIPTOR;
        //             bt_gatt_discover(conn, desc_params);
        //         }
        //     }
        //     return BT_GATT_ITER_STOP;
        // }
    }

    struct bt_gatt_service_val *service = (struct bt_gatt_service_val *)attr->user_data;

    struct gatt_service *gatt_service = k_malloc(sizeof(*gatt_service));
    if (!gatt_service)
        return BT_GATT_ITER_CONTINUE;

    // gatt_service->uuid = (struct bt_uuid *)service->uuid;
    gatt_service->uuid = k_malloc(sizeof(struct bt_uuid_128));
    if (gatt_service->uuid)
    {
        memcpy(gatt_service->uuid, service->uuid, sizeof(struct bt_uuid_128));
    }
    else
    {
        LOG_ERR("Failed to allocate memory for service UUID");
    }
    gatt_service->start_handle = attr->handle;
    gatt_service->end_handle = service->end_handle;

    printk("start_handle=%04X, end_handle=%04X\n", gatt_service->start_handle, gatt_service->end_handle);

    // **如果 `end_handle` 仍然是 0xFFFF，那就用 `prev_end_handle` 计算**
    if (prev_end_handle > 0)
    {
        struct gatt_service *last_service = CONTAINER_OF(sys_slist_peek_tail(&dev->gatt_services),
                                                         struct gatt_service, node);
        if (last_service)
        {
            last_service->end_handle = gatt_service->start_handle - 1;
        }
    }

    sys_slist_init(&gatt_service->characteristics);
    sys_slist_append(&dev->gatt_services, &gatt_service->node);

    prev_end_handle = gatt_service->start_handle;

    char uuid_str[BT_UUID_STR_LEN];
    bt_uuid_to_str(gatt_service->uuid, uuid_str, sizeof(uuid_str));
    LOG_INF("Discovered Service: UUID %s, Handle Range [0x%04X - 0x%04X]",
            uuid_str, attr->handle, service->end_handle);

    return BT_GATT_ITER_CONTINUE;
}

int bt_central_enable_notify(const bt_addr_le_t *addr, struct bt_uuid *uuid, bool enable)
{
    struct bt_device *dev = find_device(addr);
    if (!dev)
        return -ENOTCONN;

    struct gatt_service *service;
    struct gatt_characteristic *gatt_char = NULL;

    // **查找 UUID 对应的 Characteristic**
    SYS_SLIST_FOR_EACH_CONTAINER(&dev->gatt_services, service, node)
    {
        SYS_SLIST_FOR_EACH_CONTAINER(&service->characteristics, gatt_char, node)
        {
            if (!bt_uuid_cmp(gatt_char->uuid, uuid))
            {
                break;
            }
        }
    }

    if (!gatt_char)
    {
        LOG_ERR("Characteristic UUID not found");
        return -EINVAL;
    }

    if (gatt_char->ccc_handle == 0)
    {
        LOG_ERR("No CCCD handle found for characteristic");
        return -EINVAL;
    }

    memset(&gatt_char->subscribe_params, 0, sizeof(gatt_char->subscribe_params));

    gatt_char->subscribe_params.notify = notify_cb;
    gatt_char->subscribe_params.value = enable ? BT_GATT_CCC_NOTIFY : 0;
    gatt_char->subscribe_params.ccc_handle = gatt_char->ccc_handle;
    gatt_char->subscribe_params.value_handle = gatt_char->handle;

    int err = enable ? bt_gatt_subscribe(dev->conn, &gatt_char->subscribe_params)
                     : bt_gatt_unsubscribe(dev->conn, &gatt_char->subscribe_params);

    if (err)
    {
        LOG_ERR("Failed to %s Notify (err %d)", enable ? "enable" : "disable", err);
        return err;
    }

    char uuid_str[BT_UUID_STR_LEN];
    bt_uuid_to_str(uuid, uuid_str, sizeof(uuid_str));
    LOG_INF("%s Notify for UUID: %s", enable ? "Enabled" : "Disabled", uuid_str);
    return 0;
}

static struct bt_gatt_discover_params discover_params;
static struct bt_gatt_discover_params char_discover_params;
static struct bt_gatt_discover_params desc_discover_params;

static uint16_t notify_handle = 0;
static uint16_t ccc_handle = 0;

static uint8_t discover_func(struct bt_conn *conn, const struct bt_gatt_attr *attr,
                             struct bt_gatt_discover_params *params)
{
    int err;

    if (!attr)
    {
        printk("Discover complete!\n");
        (void)memset(params, 0, sizeof(params));
        return BT_GATT_ITER_STOP;
    }

    printk("[ATTRIBUTE] handle (0x%04X)%u\n", attr->handle, attr->handle);

#if 1
    switch (params->type)
    {
        case BT_GATT_DISCOVER_PRIMARY:
            {
                if (!attr)
                {
                    LOG_ERR("Not found services.");
                    return BT_GATT_ITER_STOP;
                }

                struct bt_gatt_service_val *svc_val = (struct bt_gatt_service_val *)attr->user_data;
                // cache->start_handle = attr->handle;
                // cache->end_handle = svc_val->end_handle;
                LOG_INF("service: start 0x%04x, end 0x%04x", attr->handle, svc_val->end_handle);

                memset(&char_discover_params, 0, sizeof(char_discover_params));
                char_discover_params.uuid = NULL; /* 查找所有特征 */
                char_discover_params.start_handle = attr->handle;
                char_discover_params.end_handle = svc_val->end_handle;
                char_discover_params.type = BT_GATT_DISCOVER_CHARACTERISTIC;
                char_discover_params.func = discover_func;
                // k_mutex_unlock(&gatt_cache_mutex);
                int err = bt_gatt_discover(conn, &char_discover_params);

                return BT_GATT_ITER_STOP;
            }
            break;

        case BT_GATT_DISCOVER_CHARACTERISTIC:
            {
                if (!attr)
                {
                    /* 特征发现完成 */
                    // cache->char_discovery_done = true;
                    LOG_INF("Discover char done.");
                    // if (!cache->desc_discovery_in_progress) {
                    //     cache->discovered = true;
                    //     k_mutex_unlock(&gatt_cache_mutex);
                    //     if (user_discovery_cb) {
                    //         user_discovery_cb(conn, 0);
                    //     }
                    // } else {
                    //     k_mutex_unlock(&gatt_cache_mutex);
                    // }
                    return BT_GATT_ITER_STOP;
                }

                struct bt_gatt_chrc *chrc = (struct bt_gatt_chrc *)attr->user_data;
                LOG_INF("CHAR FOUND: handle 0x%04x, properties 0x%02x", chrc->value_handle, chrc->properties);
                /* 查找支持写操作的特征 */
                if (((chrc->properties & BT_GATT_CHRC_WRITE) ||
                     (chrc->properties & BT_GATT_CHRC_WRITE_WITHOUT_RESP)))
                {
                    // cache->write_handle = chrc->value_handle;
                    LOG_INF("write handle: 0x%04x", chrc->value_handle);
                }

                if ((chrc->properties & BT_GATT_CHRC_NOTIFY) && (notify_handle == 0))
                {
                    LOG_INF("Notify char handle: 0x%04x", chrc->value_handle);

                    if (ccc_handle == 0)
                    {
                        memset(&desc_discover_params, 0, sizeof(desc_discover_params));
                        desc_discover_params.uuid = BT_UUID_GATT_CCC;
                        desc_discover_params.start_handle = chrc->value_handle + 1;
                        desc_discover_params.end_handle = 0x13;
                        desc_discover_params.type = BT_GATT_DISCOVER_DESCRIPTOR;
                        desc_discover_params.func = discover_func;
                        LOG_INF("enable CCC descriptor discover.");
                        // k_mutex_unlock(&gatt_cache_mutex);
                        int err = bt_gatt_discover(conn, &desc_discover_params);
                    }
                }

                return BT_GATT_ITER_CONTINUE;
            }
            break;

        case BT_GATT_DISCOVER_DESCRIPTOR:
            {
                if (attr)
                {
                    /* 找到描述符，判断是否为 CCC 描述符 */
                    if (!bt_uuid_cmp(attr->uuid, BT_UUID_GATT_CCC))
                    {
                        // cache->ccc_handle = attr->handle;
                        LOG_INF("CCCD handle: 0x%04x", attr->handle);
                        // k_mutex_unlock(&gatt_cache_mutex);
                        return BT_GATT_ITER_STOP;
                    }
                    // k_mutex_unlock(&gatt_cache_mutex);
                    return BT_GATT_ITER_CONTINUE;
                }
            }
            break;

        default:
            break;
    }
#else
    if (!bt_uuid_cmp(discover_params.uuid, BT_UUID_NB_DATA_SERVICE))
    {
        memcpy(&discover_uuid, BT_UUID_NB_DATA_SERVICE_NOTIFY, sizeof(discover_uuid));
        discover_params.uuid = &discover_uuid.uuid;
        discover_params.start_handle = attr->handle + 1;
        discover_params.type = BT_GATT_DISCOVER_CHARACTERISTIC;
        err = bt_gatt_discover(conn, &discover_params);
        if (err)
        {
            LOG_ERR("Discover failed (err %d)", err);
        }
    }
    else if (!bt_uuid_cmp(discover_params.uuid, BT_UUID_NB_DATA_SERVICE_NOTIFY))
    {
        memcpy(&discover_uuid, BT_UUID_GATT_CCC, sizeof(discover_uuid));
        discover_params.uuid = &discover_uuid.uuid;
        discover_params.start_handle = attr->handle + 2;
        discover_params.type = BT_GATT_DISCOVER_DESCRIPTOR;
        subscribe_params.value_handle = bt_gatt_attr_value_handle(attr);
        err = bt_gatt_discover(conn, &discover_params);
        if (err)
        {
            LOG_ERR("Discover failed (err %d)", err);
        }
    }
    else
    {
        subscribe_params.notify = NULL;
        subscribe_params.value = BT_GATT_CCC_NOTIFY;
        subscribe_params.ccc_handle = attr->handle;
        err = bt_gatt_subscribe(conn, &subscribe_params);
        if (err && err != -EALREADY)
        {
            printk("Subscribe failed (err %d)\n", err);
        }
        else
        {
            printk("[SUBSCRIBE]");
        }
        return BT_GATT_ITER_STOP;
    }
#endif
    return BT_GATT_ITER_STOP;
}

static void start_service_discovery(struct bt_conn *conn)
{
    memcpy(&discover_uuid, BT_UUID_NB_DATA_SERVICE, sizeof(discover_uuid));
    // return bt_conn_mgr_discover(conn, &discover_uuid.uuid, discovery_complete_cb);
    static const char *target_mac = "CD:C8:3B:44:3D:7F";
    return bt_conn_mgr_discover_by_mac(target_mac, &discover_uuid.uuid, discovery_complete_cb);

    memset(&discover_params, 0, sizeof(discover_params));

    memcpy(&discover_uuid, BT_UUID_NB_DATA_SERVICE, sizeof(discover_uuid));

    discover_params.uuid = &discover_uuid.uuid; // 发现所有服务
    discover_params.func = discover_func;       // discover_service_cb;
    discover_params.start_handle = 0x0001;
    discover_params.end_handle = 0xffff;
    discover_params.type = BT_GATT_DISCOVER_PRIMARY;

    int err = bt_gatt_discover(conn, &discover_params);
    if (err)
    {
        LOG_ERR("Failed to start service discovery (err %d)", err);
    }
    else
    {
        LOG_INF("Started service discovery...");
    }
}

static void mtu_updated(struct bt_conn *conn, uint16_t tx, uint16_t rx)
{
    LOG_INF("MTU updated: TX ATT MTU %d, TX ATT MTU %d", tx, rx);

    k_work_cancel_delayable(&mtu_timeout_work); // **取消超时**

    // struct bt_device *dev = find_device(bt_conn_get_dst(conn));
    // if (!dev)
    // {
    //     LOG_ERR("MTU updated: no connected device found.");
    //     return;
    // }

    // // **如果 GATT 发现尚未开始，则触发 GATT 发现**
    // if (!dev->gatt_discovery_started)
    // {
    //     start_service_discovery(dev->conn);
    // }
}

static void mtu_exchange_func(struct bt_conn *conn, uint8_t err,
                              struct bt_gatt_exchange_params *params)
{
    if (!err)
    {
        LOG_INF("MTU exchange done");

        struct bt_device *dev = find_device(bt_conn_get_dst(conn));
        if (!dev)
        {
            LOG_ERR("mtu_exchange_func: no connected device found.");
            return;
        }

        // **如果 GATT 发现尚未开始，则触发 GATT 发现**
        if (!dev->gatt_discovery_started)
        {
            start_service_discovery(dev->conn);
        }
    }
    else
    {
        LOG_WRN("MTU exchange failed (err %" PRIu8 ")", err);
    }
}

static void le_phy_updated(struct bt_conn *conn,
                           struct bt_conn_le_phy_info *param)
{
    LOG_INF("PHY updated: TX PHY %u, RX PHY %u", param->tx_phy, param->rx_phy);
    request_le_data_len_update(conn);
}

static void le_data_len_updated(struct bt_conn *conn,
                                struct bt_conn_le_data_len_info *info)
{
    LOG_INF("LE Data Length updated: TX %u bytes, RX %u bytes",
            info->tx_max_len, info->rx_max_len);
}

static bool le_param_req(struct bt_conn *conn, struct bt_le_conn_param *param)
{
    return true;
}

static void le_param_updated(struct bt_conn *conn, uint16_t interval, uint16_t latency,
                             uint16_t timeout)
{
    LOG_INF("Received connection parameter update: interval %u, latency %u, timeout %u",
            interval, latency, timeout);

    // **检查 interval 是否合理**
    if (interval < BT_GAP_INIT_CONN_INT_MIN || interval > BT_GAP_INIT_CONN_INT_MAX)
    {
        LOG_WRN("Invalid connection interval, requesting new parameters...");
        bt_conn_le_param_update(conn, BT_LE_CONN_PARAM_DEFAULT);
        return;
    }

    // **检查 timeout 是否合理**
    if (timeout < 100 || timeout > 1000)
    {
        LOG_WRN("Invalid supervision timeout, adjusting...");
        bt_conn_le_param_update(conn, BT_LE_CONN_PARAM_DEFAULT);
        return;
    }

    // **如果连接参数合适，接受新的设置**
    LOG_INF("Accepted new connection parameters");

    k_work_submit(&my_work);
}

static void request_conn_param_update(struct bt_conn *conn)
{
    struct bt_le_conn_param param = {
        .interval_min = 40, // 50ms
        .interval_max = 80, // 100ms
        .latency = 0,       // 无延迟
        .timeout = 400      // 4s 超时
    };

    int err = bt_conn_le_param_update(conn, &param);
    if (err)
    {
        LOG_ERR("Failed to request connection parameters update (err %d)", err);
    }
    else
    {
        LOG_INF("Requested connection parameters update");
    }
}

static void request_le_data_len_update(struct bt_conn *conn)
{
    int err = bt_conn_le_data_len_update(conn, NULL);
    if (err)
    {
        LOG_ERR("LE Data Length update failed (err %d)", err);
    }
    else
    {
        LOG_INF("Requested LE Data Length update");
    }
}

static void request_mtu_exchange(struct bt_conn *conn)
{
    // int err = bt_gatt_exchange_mtu(conn, NULL);
    static struct bt_gatt_exchange_params exchange_params;
    exchange_params.func = mtu_exchange_func;
    int err = bt_gatt_exchange_mtu(conn, &exchange_params);
    if (err)
    {
        LOG_ERR("MTU exchange failed (err %d)", err);
    }
    else
    {
        LOG_INF("Requested MTU exchange...");
        k_work_schedule(&mtu_timeout_work, K_SECONDS(1)); // **1 秒超时**
    }
}

static void connected(struct bt_conn *conn, uint8_t err)
{
    if (err)
    {
        if (conn)
        {
            LOG_INF("Releasing conn reference for device");
            bt_conn_unref(default_conn);
            default_conn = NULL;
        }
        LOG_ERR("Connection failed (err 0x%02x)", err);
        return;
    }

    bt_addr_le_t peer_addr;
    memcpy(&peer_addr, bt_conn_get_dst(conn), sizeof(bt_addr_le_t));

    struct bt_device *dev = find_device(&peer_addr);
    if (!dev)
    {
        if (sys_slist_len(&device_list) >= MAX_CONNECTED_DEVICES)
        {
            LOG_WRN("Max connected devices reached");
            bt_conn_disconnect(conn, BT_HCI_ERR_REMOTE_USER_TERM_CONN);
            return;
        }
        dev = k_malloc(sizeof(*dev));
        if (!dev)
        {
            LOG_ERR("Memory allocation failed for new device");
            return;
        }
        bt_addr_le_copy(&dev->addr, &peer_addr);
        if (dev->conn)
        {
            printk("dev->conn is NOT NULL.\n");
        }
        else
        {
            printk("dev->conn is NULL.\n");
        }
        dev->conn = bt_conn_ref(conn);
        dev->connected = true;
        dev->mtu_exchanged = false;
        dev->gatt_discovery_started = false;
        sys_slist_init(&dev->gatt_services);
        sys_slist_append(&device_list, &dev->node);
        LOG_INF("New device added: %02X:%02X:%02X:%02X:%02X:%02X",
                dev->addr.a.val[5], dev->addr.a.val[4], dev->addr.a.val[3],
                dev->addr.a.val[2], dev->addr.a.val[1], dev->addr.a.val[0]);
    }
    else
    {
        dev->conn = bt_conn_ref(conn);
        dev->connected = true;
        dev->mtu_exchanged = false;
        dev->gatt_discovery_started = false;
        sys_slist_init(&dev->gatt_services);
        LOG_INF("Reconnected to known device: %02X:%02X:%02X:%02X:%02X:%02X",
                dev->addr.a.val[5], dev->addr.a.val[4], dev->addr.a.val[3],
                dev->addr.a.val[2], dev->addr.a.val[1], dev->addr.a.val[0]);
    }

    LOG_INF("Connected to device: %02X:%02X:%02X:%02X:%02X:%02X",
            dev->addr.a.val[5], dev->addr.a.val[4], dev->addr.a.val[3],
            dev->addr.a.val[2], dev->addr.a.val[1], dev->addr.a.val[0]);

    if (bt_conn_mgr_add(conn) < 0)
    {
        printk("Failed to add connection mgr for conn %p\n", conn);
    }

    // **如果 MTU 交换未发生，则主动请求 MTU 交换**
    if (!dev->mtu_exchanged)
    {
        request_mtu_exchange(dev->conn);
    }
    else
    {
        LOG_INF("MTU already exchanged, skipping request.");
    }
}

static void disconnected(struct bt_conn *conn, uint8_t reason)
{
    struct bt_device *dev = find_device(bt_conn_get_dst(conn));
    if (!dev)
        return;

    char addr_str[BT_ADDR_LE_STR_LEN];
    bt_addr_le_to_str(bt_conn_get_dst(conn), addr_str, sizeof(addr_str));
    printk("Disconnected: %s, reason 0x%02x %s\n", addr_str, reason, bt_hci_err_to_str(reason));

    bt_conn_unref(default_conn);
    default_conn = NULL;

    dev->connected = false;
    dev->mtu_exchanged = false;
    dev->gatt_discovery_started = false;
    prev_end_handle = 0;

    // bt_addr_le_t peer_addr;
    // memcpy(&peer_addr, bt_conn_get_dst(conn), sizeof(bt_addr_le_t));
    // bt_conn_get_info(conn, &peer_addr);

    // **清理订阅，防止重新连接时 Notify 失败**
    struct gatt_service *service;
    struct gatt_characteristic *gatt_char;
    SYS_SLIST_FOR_EACH_CONTAINER(&dev->gatt_services, service, node)
    {
        SYS_SLIST_FOR_EACH_CONTAINER(&service->characteristics, gatt_char, node)
        {
            if (gatt_char->ccc_handle)
            {
                bt_gatt_unsubscribe(dev->conn, &gatt_char->subscribe_params);
                char uuid_str[BT_UUID_STR_LEN];
                bt_uuid_to_str(service->uuid, uuid_str, sizeof(uuid_str));
                LOG_INF("Unsubscribed notify for UUID: %s", uuid_str);
            }
        }
    }

    // **2️⃣ 释放 GATT 发现的数据**
    while (!sys_slist_is_empty(&dev->gatt_services))
    {
        struct gatt_service *service = CONTAINER_OF(sys_slist_get(&dev->gatt_services), struct gatt_service, node);

        while (!sys_slist_is_empty(&service->characteristics))
        {
            struct gatt_characteristic *ch = CONTAINER_OF(sys_slist_get(&service->characteristics), struct gatt_characteristic, node);

            while (!sys_slist_is_empty(&ch->descriptors))
            {
                struct gatt_descriptor *desc = CONTAINER_OF(sys_slist_get(&ch->descriptors), struct gatt_descriptor, node);
                k_free(desc);
            }
            k_free(ch);
        }
        k_free(service);
    }

    // sys_slist_init(&dev->gatt_services); // **清空 GATT 服务列表**
    // LOG_INF("Cleared GATT discovery state.");
    bt_conn_unref(dev->conn);
    dev->conn = NULL;
    LOG_INF("Device marked as disconnected, scheduling reconnection.");

    bt_conn_mgr_remove(conn);
    bt_conn_mgr_reset(conn);

    k_work_schedule(&reconnect_work, AUTO_RECONNECT_INTERVAL);
}

static void scan_result(const struct bt_le_scan_recv_info *info,
                        struct net_buf_simple *buf)
{
    char addr_str[BT_ADDR_LE_STR_LEN];
    int err;

    bt_addr_le_to_str(info->addr, addr_str, sizeof(addr_str));
    printk("Device found: %s (RSSI %d)\n", addr_str, info->rssi);

    char target_addr_str[BT_ADDR_LE_STR_LEN];
    snprintk(target_addr_str, sizeof(target_addr_str), "%02X:%02X:%02X:%02X:%02X:%02X",
             info->addr->a.val[5], info->addr->a.val[4], info->addr->a.val[3],
             info->addr->a.val[2], info->addr->a.val[1], info->addr->a.val[0]);
    // if (strcmp(target_addr_str, "EB:08:F2:AD:20:7F") != 0)
    // if (strcmp(target_addr_str, "FC:10:9D:A9:F5:80") != 0)
    if (strcmp(target_addr_str, "CD:C8:3B:44:3D:7F") != 0)
    {
        return;
    }

    // if (info->rssi < -40)
    // {
    //     return;
    // }

    if (bt_le_scan_stop())
    {
        return;
    }

    printk("??????????\n");

    // err = bt_conn_le_create(info->addr, BT_CONN_LE_CREATE_CONN,
    //                         BT_LE_CONN_PARAM_DEFAULT, &default_conn);
    err = bt_central_connect(info->addr);
    if (err)
    {
        printk("Create conn to %s failed (%d)\n", addr_str, err);
        bt_central_scan_start();
    }
}

static void scan_timeout(void)
{
    // TODO
}

int bt_central_connect(const bt_addr_le_t *addr)
{
    // return bt_conn_le_create(addr, BT_CONN_LE_CREATE_CONN,
    //                          BT_LE_CONN_PARAM_DEFAULT, &default_conn);
    // struct bt_conn *conn = NULL;
    int err = bt_conn_le_create(addr, BT_CONN_LE_CREATE_CONN,
                                BT_LE_CONN_PARAM_DEFAULT, &default_conn);
    if (err)
    {
        LOG_ERR("Failed to create connection (err %d)", err);
        return err;
    }

    LOG_INF("Connecting to: %02X:%02X:%02X:%02X:%02X:%02X",
            addr->a.val[5], addr->a.val[4], addr->a.val[3],
            addr->a.val[2], addr->a.val[1], addr->a.val[0]);
    return 0;
}

int bt_central_disconnect(const bt_addr_le_t *addr)
{
    // struct bt_device *dev = find_device(addr);
    // if (!dev) {
    //     LOG_WRN("Device not found");
    //     return -ENOTCONN;
    // }
    // return bt_conn_disconnect(dev->conn, BT_HCI_ERR_REMOTE_USER_TERM_CONN);
    return 0;
}

int bt_central_scan_stop(void)
{
    int err = bt_le_scan_stop();
    if (err)
    {
        LOG_ERR("Stopping scan failed (err %d)", err);
        return err;
    }
    LOG_INF("Scanning stopped");
    return 0;
}

int bt_central_scan_start(void)
{
    struct bt_le_scan_param scan_param = {
        .type = BT_HCI_LE_SCAN_ACTIVE,
        .options = BT_LE_SCAN_OPT_NONE,
        .interval = BT_GAP_SCAN_FAST_INTERVAL,
        .window = BT_GAP_SCAN_FAST_WINDOW,
    };

    int err = bt_le_scan_start(&scan_param, NULL);
    if (err)
    {
        LOG_ERR("Scanning failed to start (err %d)", err);
        return err;
    }
    LOG_INF("Scanning started");
    return 0;
}

int bt_central_init(void)
{
    bt_conn_mgr_init();

    int err = bt_enable(NULL);
    if (err)
    {
        LOG_ERR("Bluetooth init failed (err %d)", err);
        return err;
    }
    LOG_INF("Bluetooth initialized");

    bt_le_scan_cb_register(&scan_cb);
    bt_conn_cb_register(&conn_cb);
    bt_gatt_cb_register(&gatt_cb);

    k_work_init_delayable(&reconnect_work, reconnect_attempt);
    k_work_schedule(&reconnect_work, AUTO_RECONNECT_INTERVAL);
    k_work_init_delayable(&mtu_timeout_work, mtu_timeout_handler);
    k_work_init(&my_work, my_work_handler);
    return 0;
}

int main(void)
{
    bt_central_init();
    bt_central_scan_start();
    return 0;
}
